%% Identification
% David Krause
% Line Follow 2012
% March 11, 2012
%
%% Code
% Simple line following test

%% Clean up
clc
close all
clear all
drawnow
a = instrfind()
if ~isempty(a)
    fclose(a)
    clear a
end

s = serial('COM20', 'Baudrate', 115200);
fopen(s)
s

%% Parameters
% Number of ADCs
N_adc = 6;
% Maximum adc
max_adc = 255;

% Sum tests for normal line
normal_sum_min = 80;

% Center of mass target
center_mass_target = 3.5;
% Mean pwm for normal operation
pwm_mean = 14;
pwm_max = 255;
pwm_min = 0;
pwm_gain = 16;

% Number of records for ADC
N_rec_adc = 1e5;

% Loop pause
loop_pause = 0.05;

%% Run
clc
state.adc_sum = 255 * 6;
state.dir1 = 1;
state.dir2 = 1;
state.pwm1 = pwm_mean;
state.pwm2 = pwm_mean;

state.adc_rec = 0;

flight_rec.adc_reading = zeros(N_rec_adc, N_adc);
flight_rec.center_mass = zeros(N_rec_adc, N_adc);
flight_rec.pwm_setting = zeros(N_rec_adc, 2);
flight_rec.pwm_dir     = zeros(N_rec_adc, 2);
flight_rec.clock = zeros(N_rec_adc, length(clock));

disp('Let''s go!');
drawnow
pause(1);

flight_rec.clock_start = clock;


while state.adc_sum > normal_sum_min
        
    % Increment the adc_rec
    state.adc_rec = mod(state.adc_rec, N_rec_adc) + 1;
    
    % Get the ADC reading
    adc_reading = get_adc_vec(s);    
    
    state.adc_sum = sum(adc_reading);            
    % Decide on the course of action
    if (state.adc_sum > normal_sum_min)
        % Normal line
        
        % Center of mass calculation
        center_mass = sum(adc_reading .* (1 : N_adc)) / sum(adc_reading);

        % Calculate the error signal
        err_sig = (center_mass_target - center_mass) * pwm_gain;
        
        % Set pwm1
        pwm1 = pwm_mean + err_sig;
        if pwm1 > 0
            state.dir1 = 1;
        else
            state.dir1 = 0;            
        end
        state.pwm1 = round(abs(pwm1));
        
        % Set pwm2
        pwm2 = pwm_mean - err_sig;
        if pwm2 > 0
            state.dir2 = 1;
        else
            state.dir2 = 0;            
        end
        state.pwm2 = round(abs(pwm2));
    else
        % Line is gone, stop
        state.pwm1 = 0;
        state.pwm2 = 0;
        state.dir1 = 1;
        state.dir2 = 1;                
    end
    
    
    % Set the PWMs
    set_motor_pwm(s, state.dir1, state.pwm1, state.dir2, state.pwm2);
    
    % Update the flight recorder
    flight_rec.pwm_setting(state.adc_rec, :) = [state.pwm1, state.pwm2];
    flight_rec.pwm_dir(state.adc_rec, :)     = [state.dir1, state.dir2];
    flight_rec.center_mass(state.adc_rec, :) = center_mass;
    flight_rec.adc_reading(state.adc_rec, :) = adc_reading;
    flight_rec.clock(state.adc_rec, :) = clock;
    
    pause(loop_pause);
end

flight_rec.clock_stop = clock;


% Report
close all
figure
plot(flight_rec.pwm_setting(1 : state.adc_rec, 1) .* (flight_rec.pwm_dir(1 : state.adc_rec, 1) * 2 - 1), 'r')
hold on
plot(flight_rec.pwm_setting(1 : state.adc_rec, 2) .* (flight_rec.pwm_dir(1 : state.adc_rec, 2) * 2 - 1), 'g');

figure;
plot(flight_rec.center_mass(1 : state.adc_rec), 'b*-');

temp = zeros(state.adc_rec - 1, 1);
for count = 2 : state.adc_rec
    temp(count) = etime(flight_rec.clock(count, :), flight_rec.clock(count - 1, :));
end

figure
plot(temp, 'k-');

figure
plot(sum(flight_rec.adc_reading(1 : state.adc_rec, :), 2))

figure
image(flight_rec.adc_reading(1 : state.adc_rec, :));